#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>

#include "auto360_mosaic.hpp"
#include "mv_pub.hpp"
#include "mv_camera_tools.hpp"
#include "para.hpp"

#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif

using namespace cv;
using namespace std;

// 参数初始化
void Auto360Mosaic::initPara(CameraCfg &camCfg) {

}

// 按座标点生成图像
void Auto360Mosaic::makeImgWithMapPoints(Mat &srcImg, Mat &pts, Mat &dstImg) {

	int srcStep = srcImg.step;

	int dstRow = pts.size().height;
	int dstCol = pts.size().width;

	unsigned char *srcData = srcImg.ptr<unsigned char>(0);
	Point2f *curPt = pts.ptr<Point2f>(0);

	srcData[0] = srcData[1] = srcData[2] = 0; // 图像起点黑为全黑

	float p00, p01, p10, p11, pd;
	dstImg.create(pts.size().height, pts.size().width, CV_8UC3);
	unsigned char *dstData = dstImg.data;
	for (int j = 0; j < dstRow; j++) {
		for (int i = 0; i < dstCol; i++) {
			float x = curPt->x, y = curPt->y;
			int ix = x, iy = y;
			curPt++;

			// 4个点合成1个点(座标换成４个点)
			float xd = x - ix, yd = y - iy;
			float xdn = 1 + ix - x, ydn = 1 + iy - y;
			int cp = iy * srcStep + ix * 3;

			p00 = srcData[cp] * xdn;
			p01 = srcData[cp + 3] * xd;
			pd = (p00 + p01) * ydn;
			p10 = srcData[cp + srcStep] * xdn;
			p11 = srcData[cp + srcStep + 3] * xd;
			pd += (p10 + p11) * yd;
			*dstData = pd; // 目标点数据R
			dstData++;

			cp++;
			p00 = srcData[cp] * xdn;
			p01 = srcData[cp + 3] * xd;
			pd = (p00 + p01) * ydn;
			p10 = srcData[cp + srcStep] * xdn;
			p11 = srcData[cp + srcStep + 3] * xd;
			pd += (p10 + p11) * yd;
			*dstData = pd; // 目标点数据G
			dstData++;

			cp++;
			p00 = srcData[cp] * xdn;
			p01 = srcData[cp + 3] * (xd);
			pd = (p00 + p01) * ydn;
			p10 = srcData[cp + srcStep] * xdn;
			p11 = srcData[cp + srcStep + 3] * xd;
			pd += (p10 + p11) * yd;
			*dstData = pd; // 目标点数据B
			dstData++;
		}
	}
}

// 处理镜头校正
//int Auto360Mosaic::getCameraCalData(Mat &srcImg, CameraCfg &camCfg) {
//
//	if (srcImg.rows == 0 || srcImg.cols == 0) {
//		return -1;
//	}
//
//	Mat bakImg(srcImg);
//
//	// １。找校准标记点
//	vector<Point2f> markCorners;
//	MvPub mp;
//	if (mp.findChessBoard(srcImg, markCorners, 96, camCfg.findCBBlkSize, camCfg.findCBOffset) != 0) {
//		return -1;
//	}
//
//	MvCameraTools mct;
//	Point2f center(srcImg.size().width / 2, srcImg.size().height / 2);
//
//	// ２。计算标记点镜头校准后座标（座标为像素）
//	mct.getCameraCalibrationPoints(srcImg.size(), calCurv, CAL_CURV_SIZE, center, markCorners, markCorners);
//
//	// ３。计算整个图透视前座标
//	// 计算角点透视后缩放比例（角点长宽比为４：３）
//	Mat perspecPt;
//	vector<Point2f> scalCorners;
//	float upEdgeLen = 800;
//	float downEdgeLen = 800;
//	float height = 600;
//	mp.makePerspectiveCorners(upEdgeLen, downEdgeLen, height, Point2f(1.0, 8.0), scalCorners);
//
//	// 计算透视后角点座标
//	Mat edgMarkCorners;
//	mp.perspectivePosTransform(markCorners, markCorners, scalCorners, edgMarkCorners); // 求标记点变换后的座标，变换成与下边标记等宽的方形
//
//	// 计算透视后校正布最外边界４点座标（角点长宽比为４：３）
//	mp.calRectangle(edgMarkCorners, 8.0 / 5, 2.0, edgMarkCorners);
//
//	// 计算透视前校正布最外边界４点座标
//	mp.perspectivePosTransform(edgMarkCorners, scalCorners, markCorners, edgMarkCorners);
//
//	// 平移／缩放
//	upEdgeLen = camCfg.cbTopWidth / camCfg.sizePerPix;
//	downEdgeLen = camCfg.cbDownWidth / camCfg.sizePerPix;
//	height = camCfg.cbHeight / camCfg.sizePerPix;
//	Point2f downCenterPt; // 下中心
//	downCenterPt.x = camCfg.cbDownEdgeCenterPos.x / camCfg.sizePerPix;
//	downCenterPt.y = camCfg.cbDownEdgeCenterPos.y / camCfg.sizePerPix;
//	mp.makePerspectiveCorners(upEdgeLen, downEdgeLen, height, downCenterPt, scalCorners);
//
//	// 旋转透视后目标即等于旋图像
//	mp.rotatePoints(scalCorners, center, camCfg.dispRotate, scalCorners);
//
//	// 旋转后需要平移
//	Point2f offset, pos;
//	// 切换座标单位为像素
//	offset.x = camCfg.cbDownEdgeCenterPos.x / camCfg.sizePerPix - scalCorners[2].x;
//	offset.y = camCfg.cbDownEdgeCenterPos.y / camCfg.sizePerPix - scalCorners[2].y;
//	mp.movePoints(scalCorners, offset, scalCorners);
//
//	// 取得透视前的全部座标
//	mp.perspectiveInvTransform(srcImg.size(), edgMarkCorners, scalCorners, perspecPt);
//
//	// ４。计算寄变校正前座标
//	Mat preCalPt;
//	mct.cameraInvCalibration(perspecPt, calCurvInv, CAL_CURV_INV_SIZE, center, preCalPt);
//
//	// ５。生成最终图像
//	// 剪裁处理
//	Mat clipPts = preCalPt.rowRange(0, 480).clone().colRange(0, 240).clone();
//
//	// 取点
//	Mat outImg(clipPts.size().height, clipPts.size().width, CV_8UC3);
//	makeImgWithMapPoints(bakImg, clipPts, outImg);
//
//	namedWindow("atuo_360", 0);
//	imshow("atuo_360", outImg);
//	waitKey(0);
//
//	return 0;
//}

int Auto360Mosaic::findChessBoard(Mat &srcImg, CameraCfg camCfg, OutputArray markPts) {

	if (srcImg.size().height == 0 || srcImg.size().width == 0) {
		return -1;
	}

	// 找标记点
	vector<Point2f> markCorners;
	MvPub mp;
	if (mp.findChessBoard(srcImg, markCorners, 96, camCfg.findCBBlkSize, camCfg.findCBOffset) != 0) {
		return -2;
	}

	// 计算标记点镜头校准后座标（座标为像素）
	MvCameraTools mct;
	Point2f center(srcImg.size().width / 2, srcImg.size().height / 2);
	mct.getCameraCalibrationPoints(srcImg.size(), calCurv, CAL_CURV_SIZE, center, markCorners, markCorners);

	Mat o(markCorners);
	o.copyTo(markPts);

	return 0;
}

#define TARNS_AREA_HEIGHT	16
int Auto360Mosaic::makeTransMask(Size size, OutputArray mask) {

	if (size.width < TARNS_AREA_HEIGHT || size.height < TARNS_AREA_HEIGHT * 2) {
		return -1;
	}

	mask.create(size.height, size.width, CV_8UC1);
	Mat mm = mask.getMat();
	mm |= Scalar(256);

	Point r, l;	//左上角到右下角变换
	l.x = 0;
	l.y = TARNS_AREA_HEIGHT;		//浙变区域至少2*8行
	r.x = size.width - 1;
	r.y = size.height - TARNS_AREA_HEIGHT - 1;
	int mv = 128;
	int cnt = 0;
	while (true) { //中间到右边渐隐
		line(mm, l, r, Scalar(mv), 1);
		mv += (128 / TARNS_AREA_HEIGHT);
		if (mv > 256) {
			mv = 256;
		}

		if (l.y == 0) {
			l.x++;
		}
		if (l.y > 0) {
			l.y--;
		}

		if (r.y-- == 0) {
			break;
		}

		cnt++;
	}

	l.x = 0;
	l.y = TARNS_AREA_HEIGHT;		//浙变区域至少2*8行
	r.x = size.width - 1;
	r.y = size.height - TARNS_AREA_HEIGHT - 1;
	mv = 128;
	cnt = 0;
	while (true) { //中间到左边渐隐
		line(mm, l, r, Scalar(mv), 1);
		mv -= (128 / TARNS_AREA_HEIGHT);
		if (mv < 0) {
			mv = 0;
		}

		if (r.y == size.height - 1) {
			r.x--;
		}
		if (r.y < size.height - 1) {
			r.y++;
		}

		if (l.y++ > size.height - 1) {
			break;
		}

		cnt++;
	}

	return 0;
}

void Auto360Mosaic::makeTransArea(Mat &img, Mat &mask, OutputArray outImg) {

	int rows, cols;
	rows = img.size().height;
	cols = img.size().width;
	Mat o(rows, cols, CV_8UC3, Scalar(0, 0, 0));

	unsigned char *srcData = (unsigned char *) img.ptr<unsigned char *>();
	unsigned char *maskData = (unsigned char *) mask.ptr<unsigned char *>();
	unsigned char *outData = (unsigned char *) o.ptr<unsigned char *>();

	for (int j = 0; j < rows; j++) {
		for (int i = 0; i < cols; i++) {
			*outData = ((int) (*maskData) * (*srcData)) >> 8;
			outData++;
			srcData++;
			*outData = ((int) (*maskData) * (*srcData)) >> 8;
			outData++;
			srcData++;
			*outData = ((int) (*maskData) * (*srcData)) >> 8;
			outData++;
			srcData++;
			maskData++;
		}
	}

	o.copyTo(outImg);
}

// 根据已有角点生成取点数据
int Auto360Mosaic::makeCameraMapPoints(CameraCfg &camCfg, InputArray markPts, OutputArray mapPoints) {

	MvCameraTools mct;
	MvPub mp;

	Point2f center(camCfg.cameraImgSize.width / 2.0, camCfg.cameraImgSize.height / 2.0);
	Mat markCorners = markPts.getMat();

// 计算整个图透视前座标
// 计算角点透视后缩放比例（角点长宽比为５：３）
	Mat perspecPt;
	vector<Point2f> scalCorners;
	float upEdgeLen = 500;
	float downEdgeLen = 500;
	float height = 300;
	mp.makePerspectiveCorners(upEdgeLen, downEdgeLen, height, Point2f(1.0, 8.0), scalCorners);

// 计算透视后角点座标
	Mat edgMarkCorners;
	mp.perspectivePosTransform(markCorners, markCorners, scalCorners, edgMarkCorners); // 求标记点变换后的座标，变换成与下边标记等宽的方形

// 计算透视后校正布最外边界４点座标（角点长宽比为４：３）
	mp.calRectangle(edgMarkCorners, 8.0 / 5, 2.0, edgMarkCorners);

// 计算透视前校正布最外边界４点座标
	mp.perspectivePosTransform(edgMarkCorners, scalCorners, markCorners, edgMarkCorners);

// 平移／缩放
	upEdgeLen = camCfg.cbTopWidth / camCfg.sizePerPix;
	downEdgeLen = camCfg.cbDownWidth / camCfg.sizePerPix;
	height = camCfg.cbHeight / camCfg.sizePerPix;

	Point2f downCenterPt; // 下中心
	downCenterPt.x = camCfg.cbDownEdgeCenterPos.x / camCfg.sizePerPix;
	downCenterPt.y = camCfg.cbDownEdgeCenterPos.y / camCfg.sizePerPix;
	mp.makePerspectiveCorners(upEdgeLen, downEdgeLen, height, downCenterPt, scalCorners);

// 旋转透视后目标即等于旋图像
	mp.rotatePoints(scalCorners, center, camCfg.dispRotate, scalCorners);

// 旋转后需要平移
// 切换座标单位为像素
	Point2f offset;
	offset.x = camCfg.cbDownEdgeCenterPos.x / camCfg.sizePerPix - (scalCorners[2].x + scalCorners[3].x) / 2 - 0.5;
	offset.y = camCfg.cbDownEdgeCenterPos.y / camCfg.sizePerPix - (scalCorners[2].y + scalCorners[3].y) / 2 - 0.5;
	mp.movePoints(scalCorners, offset, scalCorners);

// 取得透视前的全部座标
	mp.perspectiveInvTransform(camCfg.cameraImgSize, edgMarkCorners, scalCorners, perspecPt);

// 计算寄变校正前座标
	Mat preCalPt;
	mct.cameraInvCalibration(perspecPt, calCurvInv, CAL_CURV_INV_SIZE, center, preCalPt);

// 剪裁处理
	Range row(camCfg.dispStart.y / camCfg.sizePerPix,
			camCfg.dispStart.y / camCfg.sizePerPix + camCfg.dispSize.height / camCfg.sizePerPix);
	Range col(camCfg.dispStart.x / camCfg.sizePerPix,
			camCfg.dispStart.x / camCfg.sizePerPix + camCfg.dispSize.width / camCfg.sizePerPix);
	Mat clipPts = preCalPt(row, col);

	clipPts.copyTo(mapPoints);

	return 0;
}

// 生成以校准布为低线的经过剪裁的４张图的映射表（按cm操作）
int Auto360Mosaic::makeCamerasMap(void) {

	CameraCfg tmpCfg;

	tmpCfg.dispRotate = 0.0;
	tmpCfg.cbDownEdgeCenterPos.x = getMaxDispSize(tmpCfg.sizePerPix).width / 2;
	tmpCfg.cbDownEdgeCenterPos.y = (getMaxDispSize(tmpCfg.sizePerPix).height - carSize.height) / 2;
	tmpCfg.dispStart = Point2f(0, 0);
	tmpCfg.dispSize.width = getMaxDispSize(tmpCfg.sizePerPix).width;
	tmpCfg.dispSize.height = tmpCfg.cbDownEdgeCenterPos.y;
	camCfg[0] = tmpCfg;
	makeCameraMapPoints(tmpCfg, mark[0], mapPts[0]);

	tmpCfg.dispRotate = 90.0;
	tmpCfg.cbDownEdgeCenterPos.x = (getMaxDispSize(tmpCfg.sizePerPix).width + carSize.width) / 2;
	tmpCfg.cbDownEdgeCenterPos.y = camCfg[0].cbDownEdgeCenterPos.y + frontOffset + tmpCfg.cbDownWidth / 2;
	tmpCfg.dispStart = Point2f(tmpCfg.cbDownEdgeCenterPos.x, 0);
	tmpCfg.dispSize.width = getMaxDispSize(tmpCfg.sizePerPix).width - tmpCfg.cbDownEdgeCenterPos.x;
	tmpCfg.dispSize.height = getMaxDispSize(tmpCfg.sizePerPix).height;
	camCfg[1] = tmpCfg;
	makeCameraMapPoints(tmpCfg, mark[1], mapPts[1]);

	tmpCfg.dispRotate = 180.0;
	tmpCfg.cbDownEdgeCenterPos.x = getMaxDispSize(tmpCfg.sizePerPix).width / 2;
	tmpCfg.cbDownEdgeCenterPos.y = (getMaxDispSize(tmpCfg.sizePerPix).height + carSize.height) / 2;
	tmpCfg.dispStart = Point2f(0, tmpCfg.cbDownEdgeCenterPos.y);
	tmpCfg.dispSize.width = getMaxDispSize(tmpCfg.sizePerPix).width;
	tmpCfg.dispSize.height = getMaxDispSize(tmpCfg.sizePerPix).height - tmpCfg.cbDownEdgeCenterPos.y;
	camCfg[2] = tmpCfg;
	makeCameraMapPoints(tmpCfg, mark[2], mapPts[2]);

	tmpCfg.dispRotate = 270.0;
	tmpCfg.cbDownEdgeCenterPos.x = (getMaxDispSize(tmpCfg.sizePerPix).width - carSize.width) / 2;
	tmpCfg.cbDownEdgeCenterPos.y = camCfg[0].cbDownEdgeCenterPos.y + frontOffset + tmpCfg.cbDownWidth / 2;
	tmpCfg.dispStart = Point2f(0, 0);
	tmpCfg.dispSize.width = tmpCfg.cbDownEdgeCenterPos.x;
	tmpCfg.dispSize.height = getMaxDispSize(tmpCfg.sizePerPix).height;
	camCfg[3] = tmpCfg;
	makeCameraMapPoints(tmpCfg, mark[3], mapPts[3]);

	return 0;
}

// 由４张图的映射表生成12张图的映射表(按pix操作）
int Auto360Mosaic::makeSpliceImg(Mat inImg[], OutputArray outPts) {

// 生成最终映射图
	Mat mapFinal = outPts.getMat();

	Point start;
	Size size; // 如果为浮点会４舍５入，会出现越界的情况
	Rect dstRect, srcRect;
	MvPub mp;

	// 正前图像
	start.x = mapPts[3].cols;
	start.y = 0;
	size.width = mapPts[0].cols - mapPts[3].cols - mapPts[1].cols;
	size.height = mapPts[0].rows;
	srcRect = dstRect = Rect(start, size);
	inImg[0](srcRect).copyTo(mapFinal(dstRect));
	// 前左／右图像
	if (frontTransition < mapPts[0].rows) {
		// left front
		start.x = 0;
		start.y = 0;
		size.width = mapPts[3].cols;
		size.height = mapPts[0].rows - frontTransition;
		srcRect = dstRect = Rect(start, size);
		inImg[0](srcRect).copyTo(mapFinal(dstRect));

		// 生成过度区图像left front
		Rect rectTrans = srcRect;
		rectTrans.y = size.height;
		rectTrans.height = frontTransition;
		inImg[0](rectTrans).copyTo(transImg[0]);
		inImg[3](rectTrans).copyTo(transImg[7]);

		// 生成掩码
		makeTransMask(Size(rectTrans.width, frontTransition), transMask[0]);
		mp.mirrorTrans(transMask[0], transMask[7], MV_MIRROR_HOR);
		mp.mirrorTrans(transMask[7], transMask[7], MV_MIRROR_VER);

		makeTransArea(transImg[0], transMask[0], transImg[0]);
		makeTransArea(transImg[7], transMask[7], transImg[7]);
		transImg[0] += transImg[7];

		dstRect.y = size.height;
		dstRect.height = frontTransition;
		transImg[0].copyTo(mapFinal(dstRect));

		// 生成过度区图像right front
		start.x = camCfg[1].dispStart.x / camCfg[1].sizePerPix;
		start.y = 0;
		size.width = mapPts[0].cols - start.x;
		size.height = mapPts[0].rows - frontTransition;
		srcRect = dstRect = Rect(start, size);
		inImg[0](srcRect).copyTo(mapFinal(dstRect));

		// right front过度区
		rectTrans = srcRect;
		rectTrans.x = start.x;
		rectTrans.y = size.height;
		rectTrans.height = frontTransition;
		inImg[0](rectTrans).copyTo(transImg[1]);
		rectTrans.x = 0;
		inImg[1](rectTrans).copyTo(transImg[2]);

		// 生成掩码
		makeTransMask(Size(rectTrans.width, frontTransition), transMask[1]);
		mp.mirrorTrans(transMask[1], transMask[1], MV_MIRROR_HOR);
		makeTransMask(Size(rectTrans.width, frontTransition), transMask[2]);
		mp.mirrorTrans(transMask[2], transMask[2], MV_MIRROR_VER);

		// 生成过渡区
		makeTransArea(transImg[1], transMask[1], transImg[1]);
		makeTransArea(transImg[2], transMask[2], transImg[2]);
		transImg[2] += transImg[1];

		dstRect.y = size.height;
		dstRect.height = frontTransition;
		transImg[2].copyTo(mapFinal(dstRect));
	}

	// 右图
	start.x = camCfg[1].dispStart.x / camCfg[1].sizePerPix;
	start.y = mapPts[0].rows;
	size.width = mapPts[1].cols;
	size.height = mapPts[1].rows - mapPts[0].rows - mapPts[2].rows;
	srcRect = Rect(start, size);
	srcRect = dstRect = Rect(start, size);
	srcRect.x = 0;
	inImg[1](srcRect).copyTo(mapFinal(dstRect));

	// 后图
	start.x = mapPts[3].cols;
	start.y = camCfg[2].dispStart.y / camCfg[2].sizePerPix;
	size.width = mapFinal.cols - mapPts[3].cols - mapPts[1].cols;
	size.height = mapFinal.rows - start.y;
	if (start.y + size.height > mapFinal.rows) {
		size.height = mapFinal.rows - start.y;
	}
	if (start.x + size.width > mapFinal.cols) {
		size.width = mapFinal.cols - start.x;
	}
	srcRect = dstRect = Rect(start, size);
	srcRect.x = mapPts[3].cols;
	srcRect.y = 0;
	inImg[2](srcRect).copyTo(mapFinal(dstRect));
	// 后右／左图
	if (backTransition < mapPts[2].rows) {
		// back right
		start.x = camCfg[1].dispStart.x / camCfg[1].sizePerPix;
		start.y = camCfg[2].dispStart.y / camCfg[2].sizePerPix + backTransition;
		size.width = mapFinal.cols - start.x;
		size.height = mapFinal.rows - start.y;
		srcRect = dstRect = Rect(start, size);
		srcRect.y = backTransition;
		inImg[2](srcRect).copyTo(mapFinal(dstRect));

		// 生成过度区图像back right
		Rect rectTrans = srcRect;
		rectTrans.x = 0;
		rectTrans.y = start.y - backTransition;
		rectTrans.height = backTransition;
		inImg[1](rectTrans).copyTo(transImg[3]);
		rectTrans.x = start.x;
		rectTrans.y = 0;
		inImg[2](rectTrans).copyTo(transImg[4]);

		// 生成掩码
		makeTransMask(Size(rectTrans.width, frontTransition), transMask[3]);
		mp.mirrorTrans(transMask[3], transMask[4], MV_MIRROR_HOR);
		mp.mirrorTrans(transMask[4], transMask[4], MV_MIRROR_VER);

		makeTransArea(transImg[3], transMask[3], transImg[3]);
		makeTransArea(transImg[4], transMask[4], transImg[4]);
		transImg[4] += transImg[3];

		dstRect.y = start.y - backTransition;
		dstRect.height = backTransition;
		transImg[4].copyTo(mapFinal(dstRect));

		// back left
		start.x = 0;
		start.y = camCfg[2].dispStart.y / camCfg[2].sizePerPix + backTransition;
		size.width = mapPts[3].cols;
		size.height = mapFinal.rows - start.y;
		srcRect = dstRect = Rect(start, size);
		srcRect.y = backTransition;
		inImg[2](srcRect).copyTo(mapFinal(dstRect));

		// 生成过度区图像back left
		rectTrans = srcRect;
		rectTrans.y = 0;
		rectTrans.height = backTransition;
		inImg[2](rectTrans).copyTo(transImg[5]);
		rectTrans.y = start.y - backTransition;
		inImg[3](rectTrans).copyTo(transImg[6]);

		// 生成掩码
		makeTransMask(Size(rectTrans.width, frontTransition), transMask[5]);
		mp.mirrorTrans(transMask[5], transMask[5], MV_MIRROR_VER);
		makeTransMask(Size(rectTrans.width, frontTransition), transMask[6]);
		mp.mirrorTrans(transMask[6], transMask[6], MV_MIRROR_HOR);

		makeTransArea(transImg[5], transMask[5], transImg[5]);
		makeTransArea(transImg[6], transMask[6], transImg[6]);
		transImg[6] += transImg[5];

		dstRect.y = start.y - backTransition;
		dstRect.height = backTransition;
		transImg[6].copyTo(mapFinal(dstRect));
	}

	// 左图
	start.x = 0;
	start.y = mapPts[0].rows;
	size.width = mapPts[3].cols;
	size.height = mapPts[3].rows - mapPts[0].rows - mapPts[2].rows;
	srcRect = dstRect = Rect(start, size);
	inImg[3](srcRect).copyTo(mapFinal(dstRect));

	return 0;
}
